import rclpy
from rclpy.node import Node

from general_interface.srv import GetAllInstances, GetInstance
from general_interface.msg import InstanceMessage


class InstanceManager(Node):
    def __init__(self):
        super().__init__("instance_manager")
        self.declare_parameter("instances", [])
        self.infoes = [{"name": "robot1", "code": 1}, {"name": "robot2", "code": 2}]
        self.getAllInstancesService = self.create_service(
            GetAllInstances, "get_all_instances", self.getAllInstancesCallback
        )
        self.getInstanceService = self.create_service(
            GetInstance, "get_instance", self.getInstanceService
        )

    def getAllInstancesCallback(self, request, response):
        for info in self.infoes:
            mes = InstanceMessage()
            mes.name = info["name"]
            mes.qr_code = info["code"]
            response.instances.append(mes)
        return response

    def getInstanceService(self, request: GetInstance.Request, response):
        code = request.qr_code
        for info in self.infoes:
            if info["code"] == code:
                response.name = info["name"]
                response.success = True
                return response
        response.success = False
        return response


def main(args=None):
    rclpy.init(args=args)
    node = InstanceManager()
    rclpy.spin(node)
    rclpy.shutdown()


if __name__ == "__main__":
    main()
